#include "Components/ATK_MS901M/atk_ms901m.h"
#include "Components/algorithm.h"
#include "Components/basic.h"
#include "Components/line_patrol.h"
#include "Components/motion.h"
#include "Components/motor.h"
#include "Components/oled_hardware_i2c.h"
#include "Components/servo.h"
#include "Components/simple_json.h"
#include <ti/driverlib/m0p/dl_interrupt.h>

#include <math.h>

#define TAG "Main"

float delta_seconds = 0;

void get_delta_seconds() {
  static uint32_t last_ms = 0;
  uint32_t cur_ms;
  mspm0_get_clock_ms(&cur_ms);

  delta_seconds = (cur_ms - last_ms) / 1000.0f;
  last_ms = cur_ms;
}

int main(void) {
  SYSCFG_DL_init();
  SysTick_Init();

  uart_init();
  OLED_Init();
  motion_init();
  OLED_ShowString(0, 0, "micro second Tick Test", 16);

  float left_speed = 0;
  float right_speed = 0;

  uint32_t i = 0;

  int16_t speed = 300;
  motion_speed_ctrl(speed, speed);
  while (1) {
    get_delta_seconds();
    motion_move_pool(delta_seconds);

#if 1
    right_speed = motor_get_speed(MOTOR_REAR | MOTOR_RIGHT);
    left_speed = motor_get_speed(MOTOR_REAR | MOTOR_LEFT);
    my_printf("<%d>{speed}%f,%f,%d\n", i++, left_speed, right_speed, speed);
#endif

    if (i % 2000 == 0) {
      speed = -speed;
      motion_speed_ctrl(speed, speed);
    }

    mspm0_delay_ms(1);
  }
}
